#include "FOC_run.h"
#include "ThreeHall.h"
#include "Task_function.h"
#include "Timer.h"
#include "main_user.h"
#include "SEGGER_RTT_Port.h"

#include "foc.h"
#include "foc_pid.h"
#include "AS5600.h"
#define DEBUG 1
#if DEBUG

#else

#endif
MotorState_t MotorState;

extern   logic      logicContr;
extern   TaskTime   TaskTimePare;  

extern uint32_t adc1_in0, adc1_in1, adc2_in2, adc1_in8;
extern ADC_HandleTypeDef hadc1, hadc2;
extern TIM_HandleTypeDef htim1;
	
uint32_t start,end,cycles;
float time_us;
void (* func_ptr)(ADC_HandleTypeDef *hadc) = NULL;
// 10khz  ADC注入组采样完成中断
uint32_t Predict_ThreeHallangle;
float Predict_eleangle;
void HAL_ADCEx_InjectedConvCpltCallback( ADC_HandleTypeDef *hadc)
{

	if( hadc->Instance == hadc1.Instance)
	{
		
		if(MotorState.run_state == RUNSTATE_CALIBRATING)
		{
				Offset_CurrentReading();
			  return;
		}
		
		
#if DEBUG	
		
		start = DWT->CYCCNT; // 记录起始时间
#else
										
#endif
		//HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_5);
		
		current_q_pid.ref = 0.1f;
		current_d_pid.ref = 0.0f;
			
		/***********************   1、Processing Eleangle  ***************************/
		//Predict_ThreeHallangle = ThreeHallanglecale();  
//		Predict_eleangle = (float)Predict_ThreeHallangle* 0.0000958752f;
//		Predict_eleangle = fmodf(Predict_eleangle, Two_PI);
		
		  bsp_as5600GetAngle(&Encoder_AS5600.angle);
  		//bsp_as5600_DMAGetAngle(&Encoder_AS5600.angle);
			Encoder_AS5600.angle = fmodf(Encoder_AS5600.angle + Encoder_AS5600.angle_zero_offset, Two_PI);  // fmodf 保留符号，所以我们要修正负值
			if (Encoder_AS5600.angle < 0.0f) Encoder_AS5600.angle += Two_PI;
			Encoder_AS5600.eleangle	= fmodf(Encoder_AS5600.angle * Pole, Two_PI);
      
		  Predict_eleangle = Encoder_AS5600.eleangle;

		/*********IF VF强拖判断同步电角度*********/
		//Predict_eleangle = VF_RUN_Float(600.0f, 1.0f, 6000.0f);
		//Hall_Three.Hall_State = GET_HALLState_debounce_dynamic();
		
		/***********************   2、Processing ADC  ***************************/
		
		adc1_in1 =  ADC1->JDR1;   //HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_1);
	  adc2_in2  =  ADC2->JDR1;  //HAL_ADCEx_InjectedGetValue( &hadc2, ADC_INJECTED_RANK_1);

   	foc_abc_current_i.ib  =  ((float)adc1_in1 - ADCSampPare.OffsetPhaseV_Curr)*0.004029304f;	
    foc_abc_current_i.ia  =  ((float)adc2_in2 - ADCSampPare.OffsetPhaseU_Curr)*0.004029304f;
		foc_abc_current_i.ic = - foc_abc_current_i.ia - foc_abc_current_i.ib;
		
		/*********************  Processing EventTask   *************************/
//        TaskTimePare.PWMZD_count++;
//		 if(TaskTimePare.PWMZD_count % Hall_Three.Speed_Calculate_Times == 0 ) 					// 1ms执行一次
//		 {			
//				Hall_Three_Speedcale( );  							//计算速度
//			 /****** 4、 开始计算速度 PI环路，速度环执行频率一般小于速度更新频率数倍******/
//			 if(TaskTimePare.PWMZD_count >= Hall_Three.Speed_Calculate_Times*2) 					// 2ms执行一次
//		   {
//				  TaskTimePare.PWMZD_count=0;
//					pi_spd.Fbk = Hall_Three.Speed_RPM ;  			
//					PI_Controller_conditional((p_PI_Control)&pi_spd); 		// 计算PI值，这里的速度环输出pi_spd.OutF作为电流环iq的输入
//					
//				 //对计算出的速度值进行一阶低通滤波，这里K1K2为Q10格式，因此计算后的pi_spd.OutF还是真实值
//					//pi_spd.OutF= _IQ10mpy(HallK1_RPM,pi_spd.OutF)+_IQ10mpy(HallK2_RPM,pi_spd.Out); 
//			 }
//		 }

		/***********************   FOC Algorithm   ***************************/
		Angle_Sin_Cos(Predict_eleangle, &foc_sin_cos);  //
		Clark(foc_abc_current_i, &foc_alpha_beta_i);
		Park(foc_alpha_beta_i, foc_sin_cos, &foc_dq_i);
	
		foc_dq_i.iq = Lpf(foc_dq_i.iq, foc_dq_i.iq_last,0.8f);
		foc_dq_i.iq_last = foc_dq_i.iq;
		
		foc_dq_i.id = Lpf(foc_dq_i.id, foc_dq_i.id_last,0.8f);
		foc_dq_i.id_last = foc_dq_i.id;
		
		current_q_pid.fbk = foc_dq_i.iq;
		current_d_pid.fbk = foc_dq_i.id;
		current_pid_Control(&current_q_pid);
		current_pid_Control(&current_d_pid);
		foc_dq_v.vq = current_q_pid.output;
    foc_dq_v.vd = current_d_pid.output;
		
		Reverse_Park(foc_sin_cos, foc_dq_v, &foc_alpha_beta_v);
	 /*Overmodulation prevention*/
		RevPark_Circle_Limitation_cmsis(&foc_alpha_beta_v.v_alpha, &foc_alpha_beta_v.v_beta,
																				Udc, 0.90f);
		Foc_Svpwm(foc_alpha_beta_v, &foc_pwm, Udc, PWM_PERIOD);
		
		TIM1->CCR1 = foc_pwm.pwm_u;
		TIM1->CCR2 = foc_pwm.pwm_v;
		TIM1->CCR3 = foc_pwm.pwm_w;
		
		/*********************   FOC Algorithm END ***************************/
		

		// Upload debug parameters to simulink.
//	 if(TaskTimePare.PWMZD_count==2 ) 
//		  UploadData(&mc_info);		// 7us.
		
	
#if DEBUG	
    end = DWT->CYCCNT;   // 记录结束时间
		
    cycles  = end - start; // 计算运行的 CPU 周期数
		// 处理溢出情况（如果测量过程中发生了溢出）
    if (end < start) {
				cycles = (0x100000000ULL - start) + end;  // 处理溢出
		} else {
			cycles = end - start;
		}
		time_us  = (float)cycles / (SystemCoreClock / 1e6); // 转换为微秒
		func_ptr = HAL_ADCEx_InjectedConvCpltCallback;
#else
									
#endif		
		
		//SEGGER_RTT_Port_Write();
		//JS_RTT_PLUS_SendData(); 
	}
}



//void Svpwm_Outpwm(void)
//{
//	 /*这里用Q15定点数乘法，而PWM_HalfPerMax不为定点数，
//	 所以这里其实是将Q15定点数Svpwmdq.Ta还原成-1到1的真实值然后在计算PWM占空比*/

//	 PWM_DUTY[0]= _IQmpy(PWM_HalfPerMax,Svpwmdq.Ta)+ PWM_HalfPerMax;  
//	 PWM_DUTY[1]= _IQmpy(PWM_HalfPerMax,Svpwmdq.Tb)+ PWM_HalfPerMax;
//	 PWM_DUTY[2]= _IQmpy(PWM_HalfPerMax,Svpwmdq.Tc)+ PWM_HalfPerMax;


//	 TIM1->CCR1 = PWM_DUTY[0];//PWM_DUTY[0]  (EQEPPare.JZElecTheta>>5)
//   TIM1->CCR2 = PWM_DUTY[1];//PWM_DUTY[1]
//   TIM1->CCR3 = PWM_DUTY[2] ;//PWM_DUTY[2]
//}

//IF强拖启动  通过电流环，直接给定一个id和较小的固定角速度，待电机匀速转动后HALL状态和电角度一一对应
uint32_t IF_id_RUN(uint16_t eleangle_step, uint16_t Id_ref)
{
	static uint32_t Angular_Angle = 0;
    Angular_Angle += eleangle_step;  //计算本次电角速度
	
    if(Angular_Angle >= 65536)
        Angular_Angle -= 65536;

//	pi_iq.Ref = 0;
//	pi_id.Ref = Id_ref;
	return  Angular_Angle;  //返回Q16格式的电角度
}

//IF爬坡强拖启动  通过电流环，直接给定一个iq然后匀速增加电角度
uint32_t IF_RUN(uint16_t RPM_ref, uint16_t Iq_ref, uint16_t Time_ms)
{
	  static float Angular_Speed;
	  static float Angular_Angle = 0;
	
    uint64_t Angular_Speed_ref = (uint64_t)(RPM_ref)*65536ULL/60ULL*(uint64_t)Hall_Three.Poles; 	//计算目标电角速度  Q16格式的 rad/s
    uint32_t IF_FOC_num = (uint32_t)Time_ms*1000/Hall_Three.Speed_PWM_Time;   //爬坡过程FOC中断次数
	  uint32_t Angular_Speed_step = Angular_Speed_ref / IF_FOC_num;  //每次FOC中断的爬坡电角速度步进值

    Angular_Speed += Angular_Speed_step;  //计算本次电角速度
	  if( Angular_Speed >= Angular_Speed_ref)
			Angular_Speed = Angular_Speed_ref;
			
    Angular_Angle = Angular_Angle + Angular_Speed * (float)Hall_Three.Speed_PWM_Time / 1000000;  //计算本次电角度
    if(Angular_Angle >= 65536)
        Angular_Angle -= 65536;
    uint32_t Angular_Angle_Q16 = (uint32_t)(Angular_Angle);  //将电角度转换为Q15格式

//	pi_iq.Ref = Iq_ref;
//	pi_id.Ref = 0;
	return Angular_Angle_Q16;  //返回Q16格式的电角度
}

//VF爬坡强拖启动  跳过电流环、速度环、位置环，直接给定目标Ud = 0，Uq为一个值，然后匀速增加电角度
float Angular_Speed = 0 , Angular_Angle = 0;
uint32_t Angular_Angle_Q16 = 0;
uint64_t Angular_Speed_ref;
uint32_t IF_FOC_num;
uint32_t Angular_Speed_step;

uint32_t VF_RUN(uint16_t RPM_ref, uint32_t Uq_ref, uint16_t Time_ms)
{
	
	  Angular_Speed_ref = (uint64_t)(RPM_ref)*65536ULL/60ULL*(uint64_t)Hall_Three.Poles; 	//计算目标电角速度  0-360度换算为0-65535
    IF_FOC_num = (uint32_t)Time_ms*1000/Hall_Three.Speed_PWM_Time;   //爬坡过程FOC中断次数
	  Angular_Speed_step =Angular_Speed_ref / IF_FOC_num;  //每次FOC中断的爬坡电角速度步进值

    Angular_Speed += Angular_Speed_step;  //计算本次电角速度
	  if( Angular_Speed >= Angular_Speed_ref)
			Angular_Speed = Angular_Speed_ref;
			
    Angular_Angle = Angular_Angle + Angular_Speed * (float)Hall_Three.Speed_PWM_Time / 1000000;  //计算本次电角度
    if(Angular_Angle >= 65536)
        Angular_Angle -= 65536;
    Angular_Angle_Q16 = (uint32_t)(Angular_Angle);  //将电角度转换为Q15格式

//		IparkU.Ds= 0;   									// id目标值为0
//		IparkU.Qs= Uq_ref;
	return Angular_Angle_Q16;  //返回Q16格式的电角度
}

//VF爬坡强拖启动  跳过电流环、速度环、位置环，直接给定目标Ud = 0，Uq为一个值，然后匀速增加电角度
float VF_RUN_Float(float RPM_ref, float Uq_ref, float Time_ms)
{
		static float Angular_Speed = 0 , Angular_Angle = 0;
		static float Angular_Speed_ref;
		static float IF_FOC_num;
		static float Angular_Speed_step;
	
	  Angular_Speed_ref = RPM_ref*6.28f/60.0f*Pole; 	//计算目标电角速度
    IF_FOC_num = Time_ms * 1000 / 50.0f;   //爬坡过程FOC中断次数
	  Angular_Speed_step = Angular_Speed_ref / IF_FOC_num;  //每次FOC中断的爬坡电角速度步进值

    Angular_Speed += Angular_Speed_step;  //计算本次电角速度
	  if( Angular_Speed >= Angular_Speed_ref)
			Angular_Speed = Angular_Speed_ref;
			
    Angular_Angle = Angular_Angle + Angular_Speed * 50.0f / 1000000.0f;  //计算本次电角度
    if(Angular_Angle >= Two_PI)
        Angular_Angle -= Two_PI;
   
		foc_dq_v.vd= 0;   									// id目标值为0
		foc_dq_v.vq= Uq_ref;
	 return  Angular_Angle;  //返回电角度
}
